#include "utils.h"

#ifndef M_PI
#define M_PI       3.14159265358979323846
#endif

string_vector split_string (const std::string& str, const char ch)
{
	string_vector sv;
	size_t nIndex (0), nIndexPre (0);
	while ((nIndex = str.find (ch, nIndex)) != std::string::npos)
	{
		sv.push_back (str.substr (nIndexPre, nIndex - nIndexPre));
		nIndexPre = ++nIndex;
	}
	sv.push_back (str.substr (nIndexPre));
	return sv;
}

Eigen::Quaternionf
euler2Quat (const float x, const float y, const float z, EulerOrder order)
{
	Eigen::AngleAxisf xAngle (x, Eigen::Vector3f::UnitX ());
	Eigen::AngleAxisf yAngle (y, Eigen::Vector3f::UnitY ());
	Eigen::AngleAxisf zAngle (z, Eigen::Vector3f::UnitZ ());

	Eigen::AngleAxisf* angleAxisArray[3] = {
		&xAngle, &yAngle, &zAngle
	};

	int index0 = (order & COMP0) >> SHIFT0;
	int index1 = (order & COMP1) >> SHIFT1;
	int index2 = (order & COMP2) >> SHIFT2;

	Eigen::Quaternionf q = (*angleAxisArray[index0]) * (*angleAxisArray[index1]) * (*angleAxisArray[index2]);
	return q;
}

Eigen::Quaternionf
sortedEuler2Quat (const float ang0, const float ang1, const float ang2, EulerOrder order)
{
	Eigen::Vector3f units[] = {
		Eigen::Vector3f::UnitX (),
		Eigen::Vector3f::UnitY (),
		Eigen::Vector3f::UnitZ ()
	};

	int index0 = (order & COMP0) >> SHIFT0;
	int index1 = (order & COMP1) >> SHIFT1;
	int index2 = (order & COMP2) >> SHIFT2;

	Eigen::AngleAxisf aa0 (ang0, units[index0]);
	Eigen::AngleAxisf aa1 (ang1, units[index1]);
	Eigen::AngleAxisf aa2 (ang2, units[index2]);

	Eigen::Quaternionf q = aa0 * aa1 * aa2;
	return q;
}

float a2r (float angle)
{
	return angle * M_PI / 180;
}
